/**************************************************************************
 *************************** Include Files ********************************
 **************************************************************************/

// system
#include <stdint.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include <stdbool.h>
#include <unistd.h>
#include <bits/types/struct_timespec.h>
#include <bits/time.h>
#include <time.h>
// local
#include "gtrack.h"
#include "gtrack_2d.h"
#include "gtrack_config.h"
#include "gtrack_int.h"
#include "util_factory.h"
#include "net_protocol.h"
#include "app_thread_modules.h"
// helper
#include "defines.h"
//event_detect
#if defined(EVENT_DECT)
#include "eventDetection_module.h"
#include "event_config.h"
#endif


#define USE_REAL_DATA

GTRACK_sceneryParams appSceneryParams = {
    1, //场景边界框的数量
    { { -8.0f, 9.0f, 10.0f, 700.0f, -1.0f, 3.0f } }, /* one boundary box {x1,x2,y1,y2,z1,z2} */
    0, //场景静态框的数量
    { } /* one static box {x1,x2,y1,y2,z1,z2} */
};

GTRACK_gatingParams appGatingParams = {
    16.f, //门限函数的增益
    { 8.0f, 4.0f, 2.f, 3.f } /* 长、宽、高、径向速度约束 */
};

GTRACK_mergeParams appMergeParams = {
    10U,        // 计数阈值
    2.0f,       // 距离差异阈值
    1.0f        // 速度差异阈值
};

GTRACK_allocationParams appAllocationParams = {
    5.f,        //总SNR的阈值
    1.0f,       //0.1f速度阈值, m / s
    2U,         //5u//点数阈值
    3.0f,       //聚类时点与点之间的最大距离
    1.0f        //点之间的最大速度差
};

GTRACK_measurementPoint pointCloud[GTRACK_NUM_POINTS_MAX];
GTRACK_targetDesc       targetDescr[GTRACK_NUM_TRACKS_MAX];

const char* gtrack_config_json = "gtrack_config.json";
const char* event_config_json = "event_config.json";

/* function */
/*****************************************************************************/
/**
*
* get_single_target_info  从轨迹算法输出的目标信息链表中获取单个信息的目标
*
* @param	p_sintrack_output--轨迹算法输出的单目标信息地址；
*           p_sintarget_info--上传目标信息中的单目标信息地址；
*
* @return	返回结果，0：成功；1：失败
*
* @note		None.
*
******************************************************************************/
uint32_t get_single_target_info(GTRACK_targetDesc *p_sintrack_output, SINGLE_TARGET_INFO *p_sintarget_info)
{
    // 参数检查
    if ((p_sintrack_output == NULL) || (p_sintarget_info == NULL))
    {
        printf("get_single_target_info param err.\n");
        return 1;
    }

    p_sintarget_info->id = p_sintrack_output->tid;
    p_sintarget_info->type = 0x03;          // 临时配置：小型车类型
    p_sintarget_info->length = 42;          // 临时配置：4.2米
    p_sintarget_info->width = 20;           // 临时配置：2米
    p_sintarget_info->height = 18;          // 临时配置：1.8米
    p_sintarget_info->horiz_distance = p_sintrack_output->S[0]*100;
    p_sintarget_info->vertic_distance = p_sintrack_output->S[1]*100;
    p_sintarget_info->longitude = 0;        // 临时配置：0度  
    p_sintarget_info->latitude = 0;         // 临时配置：0度  
    p_sintarget_info->altitude = 0;         // 临时配置：0米
    p_sintarget_info->lane_number = p_sintrack_output->road_id;
    p_sintarget_info->heading_angle = 0;    // 临时配置：0度
    p_sintarget_info->speed = sqrtf((p_sintrack_output->S[2]*p_sintrack_output->S[2]) + (p_sintrack_output->S[3]*p_sintrack_output->S[3])) * 3.6f;
    p_sintarget_info->acceleration = sqrtf((p_sintrack_output->S[4]*p_sintrack_output->S[4]) + (p_sintrack_output->S[5]*p_sintrack_output->S[5]));
    p_sintarget_info->RCS = 0;              // 临时配置：0dbm2
    p_sintarget_info->confidence = p_sintrack_output->score;

    return 0;

}


/*****************************************************************************/
/**
*
* get_target_info  从轨迹算法的输出中获取要上传的轨迹信息
*
* @param	p_trakoutput--轨迹算法输出信息地址；
*           p_target_info--需要上传的目标信息地址；
*
* @return	返回结果，0：成功；1：失败
*
* @note		None.
*
******************************************************************************/
uint32_t get_target_info(GtrackModuleInstance *p_trakoutput, TRACK_INFO *p_target_info)
{
    // 参数检查
    if ((p_trakoutput == NULL) || (p_target_info == NULL))
    {
        printf("get_target_info param err.\n");
        return 1;
    }

    struct timeval tv;
    GTrack_ListElem* p_tElem = NULL;
    uint32_t l_u32track_index = 0, l_u32target_index = 0;
    GtrackUnitInstance* p_Uint = NULL;
    uint32_t l_u32ret = 0;

    // 生成本地时间
    gettimeofday(&tv, NULL);
    memcpy(&p_target_info->UTC_time[0], &tv.tv_sec, 4);
    memcpy(&p_target_info->UTC_time[4], &tv.tv_usec, 4);

    // 从算法输出信息地址追踪目标对应的链表中获取单个链表的头元素
    p_tElem = gtrack_listGetFirst(&p_trakoutput->activeList);

    // 对追踪目标的链表进行遍历
    while (p_tElem != NULL) 
    {
        l_u32track_index = p_tElem->data;
        p_Uint = (GtrackUnitInstance*)p_trakoutput->hTrack[l_u32track_index];
        GTRACK_targetDesc t;
        gtrack_unitReport(p_Uint, &t);
        l_u32target_index = p_target_info->target_num;
        l_u32ret = get_single_target_info(&t, &p_target_info->target[l_u32target_index]);
        if (l_u32ret == 0)
        {
            p_target_info->target_num++;
            // 检测到的数量超过检测数量上限
            if (p_target_info->target_num >= DETECT_MAX_TARGET_NUM)
            {
                printf("Targets number exceeds the detection limit, target numner is %u.\n", p_target_info->target_num);
                break;
            }
        }
        p_tElem = gtrack_listGetNext(p_tElem);
    }

    // 检测是否有目标信息
    if (p_target_info->target_num == 0)
    {
//        printf("track output has no target info.\n");
    }

    return 0;
}

int change_data_type_from_dsp_to_gtrack(uint8_t *p_data_src,uint32_t* p_num,GTRACK_measurementPoint *p_pointCloud)
{
    int i=0;
    float r,v,a,s;
    int dst_num_cnt =0;
    object_info_type *src_data = (object_info_type *)p_data_src;

    if((p_data_src ==NULL) || (p_pointCloud ==NULL))
    {
        printf("change_data_type_from_dsp_to_gtrack err input \n");
        return 1;
    }
    else
    {
        printf("track info src_data->object_num:%d\n",src_data->object_num);
    }

    for(i=0;i<src_data->object_num;i++)
    {
 //       memset(&p_pointCloud[i],0,sizeof(GTRACK_measurementPoint));
        //距离
        r = src_data->distance[i];
        //速度
        v  = src_data->velocity[i];
        //角度
        a = src_data->azimuth[i];
        //置信度
        s = src_data->noise_SNR[i];

        if(a == -255){
  //          printf("err point a!\n");
            continue;
        }

        p_pointCloud[dst_num_cnt].array[0] = r;
        p_pointCloud[dst_num_cnt].array[1] = RAD(a);
        p_pointCloud[dst_num_cnt].array[2] = v;
        p_pointCloud[dst_num_cnt].snr = s;
        p_pointCloud[dst_num_cnt].doa_snr =s;
        dst_num_cnt++;
//        printf("r:v:a:s:%f:%f:%f:%f\n",p_pointCloud[i].array[0],p_pointCloud[i].array[2],p_pointCloud[i].array[1],p_pointCloud[i].snr);
    }
    // if(src_data->object_num > 0){
    //     r = src_data->distance[0];
    //     v = src_data->velocity[0];
    //     a = src_data->azimuth[0];
    //     s = src_data->noise_SNR[0];
    //     printf("0:r:v:a:s:%f:%f:%f:%f\n",r,v,a,s);
    
    //     r = src_data->distance[1];
    //     v = src_data->velocity[1];
    //     a = src_data->azimuth[1];
    //     s = src_data->noise_SNR[1];
    //     printf("1:r:v:a:s:%f:%f:%f:%f\n",r,v,a,s);
    // }
    *p_num = dst_num_cnt;
#if 0
    printf("num:%d:%d\n",*p_num,src_data->object_max_num);
#endif
    if(src_data->object_num == 0)
    {
        printf("[warn]:no data to gtrack!\n");
    }
    return 0;
}

int change_data_type_from_dsp_to_dspupload(uint8_t *p_data_src,UPLOAD_DSP_DATA *p_output_dsp)
{
    object_info_type *src_data = (object_info_type *)p_data_src;
    uint32_t i = 0;

    if((p_data_src == NULL) || (p_output_dsp == NULL))
    {
        printf("change_data_type_from_dsp_to_dspupload err input \n");
        return 1;
    }
    else
    {
        printf("dsp info src_data->object_num:%d\n",src_data->object_num);
    }

    p_output_dsp->total_frame_num = 0x00;
    p_output_dsp->frame_count = 0x00;
    get_utc_time(p_output_dsp->UTC_time);
    p_output_dsp->target_num = src_data->object_num;

    for(i = 0; i < src_data->object_num; i++)
    {
        p_output_dsp->dsp_single_target_info[i].index_info = src_data->index_info[i];
        p_output_dsp->dsp_single_target_info[i].noise_SNR = src_data->noise_SNR[i];
        p_output_dsp->dsp_single_target_info[i].velocity = src_data->velocity[i];
        p_output_dsp->dsp_single_target_info[i].distance = src_data->distance[i];
        p_output_dsp->dsp_single_target_info[i].azimuth = src_data->azimuth[i];
        p_output_dsp->dsp_single_target_info[i].DOA_snr = src_data->DOA_snr[i];
        p_output_dsp->dsp_single_target_info[i].Poi_peak_intense = 0x00;
        p_output_dsp->dsp_single_target_info[i].sped_resol_times = 0x00;
        p_output_dsp->dsp_single_target_info[i].Phase_Array_Vari = 0x00;

    }

    return 0;
}


void *gtarck_main_pthread(void *arg)
{
    GTRACK_moduleConfig       config;
    GTRACK_advancedParameters advParams;
    TRACK_INFO                target_info;
    UPLOAD_DSP_DATA           dsp_info;

    void* hTrackModule;
    size_t  result;
    int     n;
    int32_t errCode;
    uint32_t l_u32ret = 0;

    uint32_t gtick;
    uint32_t mNum;
    uint16_t tNum;
    GTRACK_boundaryBox* box;
      
    char rfilename[] ="/home/lds/mmv_app/data_512cp_T150ms_2.bin";
    char wfilepath[] ="/home/zzg/mmv_app/way_data";         // 目录，目录里面是一条一条的轨迹信息 距离速度角度
    char w_data_use_time[] ="/home/zzg/mmv_app/data_use_time.txt";    
    uint32_t loop_offset = 0; 
    char wfilename[256];
    uint32_t time_inv;
    uint32_t frame_cnt =0 ;
    GtrackModuleInstance* gtrakoutput;

    memset((void*)&config, 0, sizeof(GTRACK_moduleConfig));
    memset((void*)&advParams, 0, sizeof(GTRACK_advancedParameters));

    advParams.allocationParams = &appAllocationParams;
    advParams.gatingParams = &appGatingParams;
    advParams.sceneryParams = &appSceneryParams;
    advParams.mergeParams = &appMergeParams;

    config.advParams = &advParams;
    
    if (!gtrack_config_load(gtrack_config_json, (void*)&config))
    {
        gtrack_config_save(gtrack_config_json, (void*)&config);
    }



    // 事件检测定义部分
#if defined(EVENT_DECT) 
    void* eventModule;
    event_moduleConfig event_config;
    if (!event_config_load(event_config_json, (void*)&event_config))
    {
        event_config_save(event_config_json, (void*)&event_config);
    }
    eventModule = event_creatEventModuleInstace(&event_config);
    // 输出信息指针
    Traffic_info l_sInfoOutput;
    l_sInfoOutput.traffic_FlowParameters = (Traffic_FlowParameters*)gtrack_alloc(event_config.laneParams.maxNumLanes, sizeof(Traffic_FlowParameters));
    l_sInfoOutput.traffic_Jam = (Traffic_jam*)gtrack_alloc(event_config.laneParams.maxNumLanes, sizeof(Traffic_jam));
    l_sInfoOutput.traffic_Event = (Traffic_event*)gtrack_alloc(event_config.maxNumTarget, sizeof(Traffic_event));
    l_sInfoOutput.road_num = event_config.laneParams.maxNumLanes;
#endif

    printf("Tracker Configuration\n");
    printf("\tstateVectorType: %d\n", config.stateVectorType);
    printf("\tmaxNumPoints: %d\n", config.maxNumPoints);
    printf("\tmaxNumTracks: %d\n", config.maxNumTracks);
    printf("\tmaxRadialVelocity: %f\n", config.radarParams.maxRadialVelocity);
    printf("\tradialVelocityResolution: %f\n", config.radarParams.radialVelocityResolution);
    printf("\tdeltaT: %f\n", config.radarParams.deltaT);

    printf("\tinitialRadialVelocity: %f\n", config.initialRadialVelocity);
    printf("\tmaxAcceleration: [%f, %f, %f]\n", config.maxAcceleration[0], config.maxAcceleration[1], config.maxAcceleration[2]);

    printf("\tscenery:\n");
    for (n = 0; n < config.advParams->sceneryParams->numBoundaryBoxes; n++)
    {
        box = &config.advParams->sceneryParams->boundaryBox[n];
        printf("\t\t BoundaryBox %d: [%f, %f, %f, %f, %f, %f]\n", n, box->x1, box->x2, box->y1, box->y2, box->z1, box->z2);
    }
    for (n = 0; n < config.advParams->sceneryParams->numStaticBoxes; n++)
    {
        box = &config.advParams->sceneryParams->staticBox[n];
        printf("\t\t StaticBox %d: [%f, %f, %f, %f, %f, %f]\n", n, box->x1, box->x2, box->y1, box->y2, box->z1, box->z2);
    }
    printf("\tallocation: [%f, %f, %d, %f, %f]\n", config.advParams->allocationParams->snrThre, config.advParams->allocationParams->velocityThre, config.advParams->allocationParams->pointsThre, config.advParams->allocationParams->maxDistanceThre, config.advParams->allocationParams->maxVelThre);
    printf("\tgating: [%f, %f, %f, %f, %f]\n", config.advParams->gatingParams->gain, config.advParams->gatingParams->limitsArray[0], config.advParams->gatingParams->limitsArray[1], config.advParams->gatingParams->limitsArray[2], config.advParams->gatingParams->limitsArray[3]);

    // 创建gtracker对象并初始化
    hTrackModule = gtrack_create(&config, &errCode);

    gtrakoutput = (GtrackModuleInstance*)hTrackModule;

    while (1)
    {
#ifdef USE_REAL_DATA
        // 等待dsp输出目标信息数据的结果
        // sem_wait(&g_track_process_semaphore);
        sem_wait(&g_dsp_pro_comlete_semaphore);
        l_u32ret = change_data_type_from_dsp_to_gtrack(g_obj_info_pu8_mmap,&mNum,pointCloud);
        if(l_u32ret){
            printf("change_data_type_from_dsp_to_gtrack err!\n");
            continue;
        }

        if (g_dsp_req_info.enable_flag == 1)    // upload dsp info
        {
            l_u32ret = change_data_type_from_dsp_to_dspupload(g_obj_info_pu8_mmap, &dsp_info);
            if(l_u32ret){
                printf("change_data_type_from_dsp_to_dspupload err!\n");
                continue;
            }
        }
        
#else

        // result = fh.readPcFile(pointCloud, mNum, gtick);
        // if (result != 1)
        // {
        //     break;
        // }

        result = read_file(rfilename,&gtick,loop_offset,sizeof(uint32_t),1);
        if(result){
            printf("read gtick end!\n");
            break;
        }
        result = read_file(rfilename,&mNum,loop_offset+1*sizeof(uint32_t),sizeof(uint32_t),1);
        if(result){
            printf("read numOfCur end!\n");
            break;
        } 
//        printf("%d,%d\n",gtick,numOfCur);
        result = read_file(rfilename,pointCloud,loop_offset+2*sizeof(uint32_t),sizeof(GTRACK_measurementPoint),mNum);
        if(result){
            printf("read pointCloud end:offset:%x!\n",loop_offset);
            break;
        }

        loop_offset = loop_offset + sizeof(uint32_t)+sizeof(uint32_t)+mNum*sizeof(GTRACK_measurementPoint);
        // printf("gtick:%d,mNum:%d,loop_offset addr:%x\n",gtick,mNum,loop_offset);
        // Limit the points


#endif
        if (mNum > config.maxNumPoints)
        mNum = config.maxNumPoints;

        // gtrack算法函数入口
        gtrack_step(hTrackModule, pointCloud, 0, mNum, targetDescr, &tNum, 0);
#if defined(EVENT_DECT)
        event_Step(eventModule, targetDescr, &tNum, &l_sInfoOutput);
#endif

        // 获取需要上传的目标信息
        memset(&target_info, 0x00, sizeof(target_info));
        l_u32ret = get_target_info(gtrakoutput, &target_info);
        if (l_u32ret != 0)
        {
            continue;
        }

        // 将目标轨迹信息存到队列当中
        l_u32ret = add_to_queue((const void *)&target_info, sizeof(target_info), &g_track_info_queue);
        if (l_u32ret != 0)
        {
            continue;
        }

        if (g_dsp_req_info.enable_flag == 1)    // upload dsp info
        {
            // 将目标轨迹信息存到队列当中
            l_u32ret = add_to_queue((const void *)&dsp_info, sizeof(dsp_info), &g_dsp_info_queue);
            if (l_u32ret != 0)
            {
                continue;
            }
        }

#ifdef USE_REAL_DATA 
#else      
        // 测试使用
        usleep(1000 * 100);
#endif        

//        // 获取需要上传的目标信息
//         usleep(1000);

//         char tmp[50];
//         memset(tmp,0,sizeof(tmp));
//         sprintf(tmp,"%d,%d,%d\n",mNum,gtrakoutput->targetNumCurrent,time_inv);
//         write_file(w_data_use_time,tmp,strlen(tmp),1);
//         /**********************计算耗时end******************** */

//         memset(wfilename,0,sizeof(wfilename));       
//         creat_file_name(wfilepath,1,gtick,wfilename);
//  //       write_file(wfilename,&gtick,sizeof(uint32_t),1);
//  //       printf("name:%s\n",wfilename);  
//          GTrack_ListElem* tElem = gtrack_listGetFirst(&gtrakoutput->activeList);
// 		while (tElem != NULL) {
// 			uint8_t uid = tElem->data;
// 			GtrackUnitInstance* gUint = (GtrackUnitInstance*)gtrakoutput->hTrack[uid];
// 			GTRACK_targetDesc t;
// 			gtrack_unitReport(gUint, &t);
//  //           printf("get way。\n");
//             tElem = gtrack_listGetNext(tElem);
//             write_file(wfilename,&t,sizeof(GTRACK_targetDesc),1);
//         }
//         printf("GTRACK_targetDesc:float:%d,%x,tNum:%d\n",sizeof(float),sizeof(GTRACK_targetDesc),tNum);
//         usleep(1000);
//         printf("0x%8x:%.2x,%.2x",(uint8_t *)(gtrakoutput->targetDesc),*(uint8_t *)(gtrakoutput->targetDesc),*((uint8_t *)(gtrakoutput->targetDesc)+1));
    }
    return (void *)1;
}
